#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include<Wire.h>
#include <ros.h>
#include <std_msgs/String.h>
#include "sensor_msgs/JointState.h"
#include <std_msgs/Empty.h>





//the setting of pwm servo
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//double servo_thread[8];

ros::NodeHandle  nh;

void stand(){
  Serial.println("stand");
  int x=1;
  while(x==1){
  setServoPulse(0,0.0005);
  setServoPulse(4,0.0025);
  setServoPulse(8,0.0005);
  setServoPulse(12,0.0025);
 delay(2000);
 
 setServoPulse(0,0.001);
 setServoPulse(2,0.0005);
 setServoPulse(4,0.0020);
 setServoPulse(6,0.0025);
 setServoPulse(8,0.00094);
 setServoPulse(10,0.0005);
 setServoPulse(12,0.00206);
 setServoPulse(14,0.0025);
 delay(2000);
 x=x+1;}
  
}

void walk(){
  
}

void robotstatesCallback(const std_msgs::String& msg){
  if (msg.data=="stand") {
    Serial.println(msg.data);
    stand();
  }
    else  walk();
}

/*void jointstatesCallback(const sensor_msgs::JointState& msg){
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  servo_thread[0]=msg->position[0];
  set_joint_thread(servo_thread);
}*/

ros::Subscriber<std_msgs::String> sub("robot_states", robotstatesCallback);







//-------------------------------------------------the place of all funcation's init 
void setup(){

    Serial.begin(115200);

//the part of pwm's setup
  pwm.begin();
  pwm.setOscillatorFrequency(25000000);
  pwm.setPWMFreq(50);

  nh.initNode();
  nh.subscribe(sub);
  
}

//---------------------








//------------------------------------------------------------------------------------------------------------------------the place of all funcation's init
//------------------------------------------------the funcation of controling servo by pwm
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 50;   // Analog servos run at ~60 Hz updates 
  pulselength /= 4096;  // 12 bits of resolution
  pulse *= 1000000;  // convert input seconds to us
  pulse /= pulselength;
  pwm.setPWM(n, 0, pulse);
}
void set_joint_thread(double thread[]){
  setServoPulse(0,thread[0]); 
  setServoPulse(2,thread[1]); 
  setServoPulse(4,thread[2]); 
  setServoPulse(6,thread[3]); 
  setServoPulse(8,thread[4]); 
  setServoPulse(10,thread[5]); 
  setServoPulse(12,thread[6]); 
  setServoPulse(14,thread[7]); 
}
//------------------------------------------------the funcation of controling servo by pwm

int num=0;

void loop(){


  

}
